//
// = FILENAME
//    moveMotor.cc
//
// = AUTHOR(S)
//    Patric Jensfelt
//    
// = COPYRIGHT
//    Copyright (c) 2010 Patric Jensfelt
//
/*----------------------------------------------------------------------*/

#include "Serializer.hh"
#include <iostream>
#include <csignal>

bool run = true;
void sigtrap(int) { run = false; }

int main()
{
  Serializer s;

  s.connect("/dev/ttyS2");

  s.setUnit(Serializer::UNIT_RAW);
  s.setEncoderType(Serializer::ENCODER_QUAD);
  s.clearEncoders();

  signal(SIGINT, sigtrap);

  std::cout << "\nPress ctrl-c to stop the program\n\n";
  double pwm = 0;
  bool increase = true;
  while (run) {
    double t0 = s.getCurrentTime();
    s.setMotorPWMs(pwm, pwm);
    double t1 = s.getCurrentTime();
    int speed0, speed1;
    s.getMotorSpeeds(speed0, speed1);
    double t2 = s.getCurrentTime();
    long enc0, enc1;
    s.getEncoders(enc0, enc1);
    double t3 = s.getCurrentTime();
    std::cout << "dt1=" << t1-t0 << " dt2=" << t2-t1 << " dt3=" << t3-t2
              << " pwm=" << pwm 
	      << " speed0=" << speed0 << " speed1=" << speed1 
              << " enc0=" << enc0 << " enc1=" << enc1
	      << std::endl;

    if (increase) {
      pwm++;
      if (pwm > 100) {
	pwm = 99;
        increase = false;
      }
    } else {
      pwm--;
      if (pwm < -100) {
	pwm = -99;
        increase = true;
      }
    }
  }

  s.stopAllMotors();
  s.disconnect();

  return 0;
}
